#include "Arduino.h"
#include "ACCEL_v1.h"
#include "BMA180.h"
#include "Wire.h"

BMA180::BMA180(void) {
   _accADDR = 0x40;    // I2C Bus Address of BMA180 Accelerometer
   _aRANG = 0x34;    // BMA180 Range Register Word Address
   _aACCL = 0x02;    // BMA180 Acceleration Register Base
   _aBAND = 0x20;    // BMA180 BW Register Address
   _aEEWR = 0x20;    // BMA180 EEPROM Write register offset
   _aRMSK = 0x0E;    // Range bit mask
   _aBMSK = 0xF0;    // BW bit mask
   _aECTL = 0x0D;    // ee_w control register
   _aEE_W = 0x10;    // set ee_w bit
   x_bias = 0;
   y_bias = 0;
   z_bias = 0;
}

void BMA180::get_accel(void) {
 
  Wire.beginTransmission(_accADDR); // transmit to Accelerometer
  Wire.write(_aACCL);        // Send A/D accel data register address
  Wire.endTransmission();    // stop transmitting
  
  Wire.requestFrom(_accADDR, 6);
  
  byte LSB = Wire.read();  // recieve LSB
  byte MSB = Wire.read();
  x_accel = ((int(MSB) << 8) + LSB) >> 2;
  x_accel += x_bias;
  
  LSB = Wire.read();  // recieve LSB
  MSB = Wire.read();
  y_accel = ((int(MSB) << 8) + LSB) >> 2;
  y_accel += y_bias;
  
  LSB = Wire.read();  // recieve LSB
  MSB = Wire.read();
  z_accel = ((int(MSB) << 8) + LSB) >> 2;
  z_accel += z_bias;

}

void BMA180::set_gain(byte gain) {

  // Read current value of range register & even register before it
  Wire.beginTransmission(_accADDR); // transmit to Altimeter
  Wire.write(_aRANG);        // Send range register address - 1
  Wire.endTransmission();    // stop transmitting
  //delay(1);

  Wire.requestFrom(_accADDR, 2);
  byte P = Wire.read();  // get even register for safety
  byte R = Wire.read();  // recieve Range Register
          
  // Set Range bits to gain
  R = (R & ~_aRMSK) | ((gain << 1) & _aRMSK);
          
  // Enable image writing
  _write_lock(false);
  // Write Back to BMA180
  Wire.beginTransmission(_accADDR); // transmit to Altimeter
  Wire.write(_aRANG + 1);        // Send Gain control register address
  Wire.write(R);
  Wire.endTransmission();    // stop transmitting
  // Lock Image Writing
  _write_lock(true);
}

int BMA180::get_gain(void) {

  // Read current value of range register & even register before it
  Wire.beginTransmission(_accADDR); // transmit to Altimeter
  Wire.write(_aRANG);        // Send range register address - 1
  Wire.endTransmission();    // stop transmitting

  Wire.requestFrom(_accADDR, 2);
  byte P = Wire.read();  // get even register for safety
  byte gain = (Wire.read() & _aRMSK) >> 1;

  return gain;
}

void BMA180::set_bw(byte bw) {

  // Read current value of bw register
  Wire.beginTransmission(_accADDR); // transmit to Altimeter
  Wire.write(_aBAND);        // Send range register address - 1
  Wire.endTransmission();    // stop transmitting

  Wire.requestFrom(_accADDR, 1);
          
  // Set BW bits to bw
  byte B = (Wire.read() & ~_aBMSK) | ((bw << 4) & _aBMSK);
          
  // Enable image writing
  _write_lock(false);
  // Write Back to BMA180
  Wire.beginTransmission(_accADDR); // transmit to Altimeter
  Wire.write(_aBAND);        // Send A/D control register address
  Wire.write(B);
  Wire.endTransmission();    // stop transmitting
  // Lock Image Writing
  _write_lock(true);
}

byte BMA180::get_bw(void) {

  // Read current value of range register & even register before it
  Wire.beginTransmission(_accADDR); // transmit to Altimeter
  Wire.write(_aBAND);        // Send bw register address - 1
  Wire.endTransmission();    // stop transmitting

  Wire.requestFrom(_accADDR, 1);
  byte bw = (Wire.read() & _aBMSK) >> 4;

  return bw;
}

void BMA180::ee_write_gain(void) {
  // Read current value of even register before range
  Wire.beginTransmission(_accADDR); // transmit to Altimeter
  Wire.write(_aRANG);        // Send range register address - 1
  Wire.endTransmission();    // stop transmitting

  Wire.requestFrom(_accADDR, 1);
  byte P = Wire.read();  // get even register for safety
          
  // Enable image writing
  _write_lock(false);
  // Transfer range settings into EEPROM
  Wire.beginTransmission(_accADDR);
  Wire.write(_aRANG + _aEEWR);
  Wire.write(P);  // Send the even register - believe this is not used
  Wire.endTransmission();
  // Lock Image Writing
  _write_lock(true);
}

void BMA180::ee_write_bw(void) {  
          
  // Read current value of bw register
  Wire.beginTransmission(_accADDR); // transmit to Altimeter
  Wire.write(_aBAND);        // Send range register address - 1
  Wire.endTransmission();    // stop transmitting

  Wire.requestFrom(_accADDR, 1);
  byte B = Wire.read();  // get even register for safety
          
  // Transfer range settings into EEPROM
  // Enable image writing
  _write_lock(false);
  Wire.beginTransmission(_accADDR);
  Wire.write(_aBAND + _aEEWR);
  Wire.write(B);  // Send the register - believe this is not used
  Wire.endTransmission();
  _write_lock(true);
}

void BMA180::_write_lock(boolean lock) {
  // Enable image writing
  int temp_set = _aEE_W;
  if (lock) temp_set = 0;
  Wire.beginTransmission(_accADDR); // transmit to Altimeter
  Wire.write(_aECTL);  // ee_w control register
  Wire.write(temp_set);  // set ee_w bit
  Wire.endTransmission();
}

